#include "Polea.h"
#include <iostream>

Polea::Polea(double x, double y, double ancho, double alto, string pathImagen) :
		Elemento(x, y, ancho, alto, pathImagen) {
	// Creacion de la polea
	this->posSogaIzqX = 0;
	this->posSogaIzqY = 0;
	this->sogaDerUnida = NULL;
	this->sogaIzqUnida = NULL;
	this->posSogaDerX = ancho;
	this->posSogaDerY = 0;
	this->sogaIzqInicial = false;
	this->sogaDerInicial = false;
	this->elementoSogaIzq = NULL;
	this->elementoSogaDer = NULL;

	//se utiliza en casos de poleas multiples, para recorrer de manera recursiva todos los nodos
	this->ajointearIzquierdo = NULL;
	this->ajointearDerecho = NULL;

}

Polea::~Polea() {
}

void Polea::referenciarElementos(){

	if (this->sogaIzqUnida!=NULL){
		pair<Elemento*,Elemento*> parElementos = this->sogaIzqUnida->obtenerExtremos();
		cout << "Elemento 1 izq: " << parElementos.first->getTipo() << endl;
		cout << "Elemento 2 izq: " << parElementos.second->getTipo() << endl;
		if(parElementos.first == this){
			this->elementoSogaIzq = parElementos.second;
		} else {
			this->elementoSogaIzq = parElementos.first;
		}
	}

	if (this->sogaDerUnida!=NULL){
		pair<Elemento*,Elemento*> parElementos = this->sogaDerUnida->obtenerExtremos();
		cout << "Elemento 1 der: " << parElementos.first->getTipo() << endl;
		cout << "Elemento 2 der: " << parElementos.second->getTipo() << endl;
		if(parElementos.first == this){
			this->elementoSogaDer = parElementos.second;
		} else {
			this->elementoSogaDer = parElementos.first;
		}
	}
}

pair<Elemento*,Elemento*>* Polea::getElementosSoga(){
	pair<Elemento*,Elemento*>* par = new pair<Elemento*,Elemento*>();
	par->first = this->elementoSogaIzq;
	par->second = this->elementoSogaDer;

	return par;
}
/*
void Polea::jointearElemento(b2World* world) {

	this->referenciarElementos();
*/
/*
	if (this->elementoSogaDer->getTipo().compare("polea")==0){
		Polea* polea = (Polea*)this->elementoSogaDer;
		this->elementoSogaDer = polea->getElementosSoga()->second;
	}

	if (this->elementoSogaIzq->getTipo().compare("polea")==0){
		Polea* polea = (Polea*)this->elementoSogaIzq;
		this->elementoSogaIzq = polea->getElementosSoga()->first;
	}
*/
/*	if (this->elementoSogaIzq && this->elementoSogaDer){

		this->obtenerExtremosConElementos(this->elementoSogaIzq,this->elementoSogaDer);

		cout << "creo polea" << endl;

		if (this->ajointearIzquierdo){
			cout << "Elementos referenciados, izq: " <<this->ajointearIzquierdo->getTipo()<<endl;
		}

		if (this->ajointearDerecho){
			cout << "Elementos referenciados, der: " <<this->ajointearDerecho->getTipo()<<endl;
		}



		EntidadFisica* cargaInicial = this->ajointearIzquierdo->getEntidadFisica();

		EntidadFisica* cargaFinal = this->ajointearDerecho->getEntidadFisica();

		PoleaFisica* poleaFisica = (PoleaFisica*)this->getEntidadFisica();

		poleaFisica->setearElementosAJointear(cargaInicial, cargaFinal);

		poleaFisica->jointear(world);

	}else{
		cout << "La polea no tiene dos elementos jointeados" <<  endl;
	}
}
*/

void Polea::jointearElemento(b2World* world) {

		this->referenciarElementos();


		if (this->elementoSogaIzq && this->elementoSogaDer){

			if (this->elementoSogaDer->getTipo().compare("zpolea")==0){
				Polea* polea = (Polea*)this->elementoSogaDer;

				cout<<"soy soga con polea a derecha"<<endl;
					polea->referenciarElementos();
					this->elementoSogaDer = polea->getElementosSoga()->second;
					pair<int,int> parPosicionesDerecha = pair<int,int>(polea->getEntidadFisica()->obtenerPosicionX(), polea->getEntidadFisica()->obtenerPosicionY());
					this->elementoSogaDer->getEntidadFisica()->setAnclajePolea(parPosicionesDerecha);
					pair<int,int> parPosicionesIzquierda = pair<int,int>(this->getEntidadFisica()->obtenerPosicionX(), this->getEntidadFisica()->obtenerPosicionY());
					this->elementoSogaIzq->getEntidadFisica()->setAnclajePolea(parPosicionesIzquierda);
					PoleaFisica* poleaFisica = (PoleaFisica*)this->getEntidadFisica();
					poleaFisica->agregarEntidadDerecha(polea->sogaDerUnida->getEntidadFisica());
					poleaFisica->agregarEntidadIzquierda(this->sogaIzqUnida->getEntidadFisica());
			}

			//yo apunto a 2, a una uqe tiene polea y a otra q no, no estoy teniendo en cuenta a la que no tiene..
			else if (this->elementoSogaIzq->getTipo().compare("zpolea")==0){
				Polea* polea = (Polea*)this->elementoSogaIzq;
				cout<<"soy soga con polea a izquierda"<<endl;
					polea->referenciarElementos();
					this->elementoSogaIzq = polea->getElementosSoga()->first;
					pair<int,int> parPosicionesIzquierda = pair<int,int>(polea->getEntidadFisica()->obtenerPosicionX(), polea->getEntidadFisica()->obtenerPosicionY());
					this->elementoSogaIzq->getEntidadFisica()->setAnclajePolea(parPosicionesIzquierda);
					pair<int,int> parPosicionesDerecha = pair<int,int>(this->getEntidadFisica()->obtenerPosicionX(), this->getEntidadFisica()->obtenerPosicionY());
					this->elementoSogaDer->getEntidadFisica()->setAnclajePolea(parPosicionesDerecha);
					PoleaFisica* poleaFisica = (PoleaFisica*)this->getEntidadFisica();
					poleaFisica->agregarEntidadDerecha(this->sogaDerUnida->getEntidadFisica());
					poleaFisica->agregarEntidadIzquierda(polea->sogaIzqUnida->getEntidadFisica());
			}

			else{
				pair<int,int> parPosicionesDerecha = pair<int,int>(this->getEntidadFisica()->obtenerPosicionX(), this->getEntidadFisica()->obtenerPosicionY());
				this->elementoSogaIzq->getEntidadFisica()->setAnclajePolea(parPosicionesDerecha);
				pair<int,int> parPosicionesIzquierda = pair<int,int>(this->getEntidadFisica()->obtenerPosicionX(), this->getEntidadFisica()->obtenerPosicionY());
				this->elementoSogaDer->getEntidadFisica()->setAnclajePolea(parPosicionesIzquierda);
				PoleaFisica* poleaFisica = (PoleaFisica*)this->getEntidadFisica();
				poleaFisica->agregarEntidadDerecha(this->sogaDerUnida->getEntidadFisica());
				poleaFisica->agregarEntidadIzquierda(this->sogaIzqUnida->getEntidadFisica());

			}

			cout << "creo polea" << endl;

			EntidadFisica* entidadIzquierda = this->elementoSogaIzq->getEntidadFisica();
			EntidadFisica* entidadDerecha = this->elementoSogaDer->getEntidadFisica();
			PoleaFisica* poleaFisica = (PoleaFisica*)this->getEntidadFisica();
//			poleaFisica->agregarEntidadDerecha(this->sogaDerUnida->getEntidadFisica());
//			poleaFisica->agregarEntidadIzquierda(this->sogaIzqUnida->getEntidadFisica());
			poleaFisica->setearElementosAJointear(entidadIzquierda, entidadDerecha);
			poleaFisica->jointear(world);
		}else{
			cout << "La polea no tiene dos elementos jointeados" <<  endl;
		}
	}





bool Polea::obtenerExtremosConElementos(Elemento* elementoAVisitar){

	if (elementoAVisitar != NULL){

		if (!elementoAVisitar->fueVisitadoEnCrear()){

			Elemento* elementoIzquierdo = elementoAVisitar->obtenerExtremos().first;
			Elemento* elementoDerecho = elementoAVisitar->obtenerExtremos().second;


			elementoAVisitar->setFueVisitadoCrear(true);

			bool izda = obtenerExtremosConElementos(elementoIzquierdo);

			bool dcha = obtenerExtremosConElementos(elementoDerecho);

			if ((elementoAVisitar->getTipo().compare("zpolea")!=0) && (elementoAVisitar->getTipo().compare("soga")!=0)){
				return true;
			}



			return (izda || dcha);
		}
	}
	return false;
}


void Polea::rotar(double angulo) {
}


bool Polea::aceptaSoga(){
	if (this->sogaIzqUnida && this->sogaDerUnida)
		return false;
	return true;
}

void Polea::dibujar(double x, double y, double ancho, double alto,SDL_Renderer* renderer){
	Elemento::dibujar(x,y,ancho,alto,renderer);
}

void Polea::actualizarValoresFisicos() {
	Elemento::actualizarValoresFisicos();
}


void Polea::activarSogaPosInicial() {
	this->sogaIzqInicial = true;
}
void Polea::activarSogaPosFinal() {
	this->sogaDerInicial = true;
}

void Polea::mover(double centroX, double centroY) {
	Elemento::mover(centroX, centroY);
	this->actualizarReferencias();
}

void Polea::actualizarReferencias() {

	if (this->sogaIzqUnida != NULL) {

//		cout << "Actualizo soga Izq" << endl;

		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(0);

		if (this->sogaIzqInicial) {

//			cout << "SOGA IZQ INICIAL" << endl;
			this->sogaIzqUnida->setPosicionInicial(
					parXYAObtener.first,
					parXYAObtener.second);
		} else {
//			cout << "NOO SOGA IZQ INICIAL" << endl;
			this->sogaIzqUnida->setPosicionFinal(
					parXYAObtener.first,
					parXYAObtener.second);
		}
	}
	if (this->sogaDerUnida != NULL) {

//		cout << "Actualizo soga Der" << endl;

		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(1);

		if (this->sogaDerInicial) {
//			cout << "SOGA DER INICIAL" << endl;

			this->sogaDerUnida->setPosicionInicial(
					parXYAObtener.first,
					parXYAObtener.second);
		} else {
//			cout << "SOGA NOOO DER INICIAL" << endl;

			this->sogaDerUnida->setPosicionFinal(
					parXYAObtener.first,
					parXYAObtener.second);
		}
	}
}

void Polea::agregarSogaInicial(Elemento* soga) {


	if (this->sogaIzqUnida) {
		cout << "Habia simulado anteriormente izq " << endl;
		if (this->sogaIzqUnida->getIdElemento() == soga->getIdElemento()) {
			pair<double,double> parXYAObtener = this->obtenerPosicionSoga(0);
			soga->setPosicionInicial(parXYAObtener.first,parXYAObtener.second);
			return;
		}
	}

	if (this->sogaDerUnida) {
		cout << "Habia simulado anteriormente der" << endl;
		if (this->sogaDerUnida->getIdElemento() == soga->getIdElemento()) {
			pair<double,double> parXYAObtener = this->obtenerPosicionSoga(1);
			soga->setPosicionInicial(parXYAObtener.first,parXYAObtener.second);
			return;
		}
	}

	// No habia simulado previamente
	pair<double, double> posicionInicioSoga = soga->getPosicionInicial();
	double medioPolea = this->getX() + this->getAncho() / 2;
	cout << "medio Polea: " << medioPolea << "y posicion inicio soga x:  "
			<< posicionInicioSoga.first << endl;

	if (posicionInicioSoga.first <= medioPolea) {
		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(0);
		soga->setPosicionInicial(parXYAObtener.first,parXYAObtener.second);
		this->sogaIzqUnida = soga;
		this->sogaIzqInicial = true;
	} else {
		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(1);
		soga->setPosicionInicial(parXYAObtener.first,parXYAObtener.second);
		this->sogaDerUnida = soga;
		this->sogaDerInicial = true;
	}

}

void Polea::agregarSogaFinal(Elemento* soga) {

	if (this->sogaIzqUnida) {
		if (this->sogaIzqUnida->getIdElemento() == soga->getIdElemento()) {
			pair<double,double> parXYAObtener = this->obtenerPosicionSoga(0);
			soga->setPosicionFinal(parXYAObtener.first,parXYAObtener.second);
			this->sogaIzqUnida = soga;
			this->sogaIzqInicial = false;
			return;
		}
	}

	if (this->sogaDerUnida) {
		if (this->sogaDerUnida->getIdElemento() == soga->getIdElemento()) {
			pair<double,double> parXYAObtener = this->obtenerPosicionSoga(1);
			soga->setPosicionFinal(parXYAObtener.first,parXYAObtener.second);
			this->sogaDerUnida = soga;
			this->sogaDerInicial = false;
			return;
		}
	}

	cout << "agregar soga final polea " << endl;
	pair<double, double> posicionFinSoga = soga->getPosicionFinal();
	double medioPolea = this->getX() + this->getAncho() / 2;
	cout << "medio Polea: " << medioPolea << "y posicion inicio soga x:  "
			<< posicionFinSoga.first << endl;

	if (posicionFinSoga.first <= medioPolea) {
		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(0);
		soga->setPosicionFinal(parXYAObtener.first,parXYAObtener.second);
		this->sogaIzqUnida = soga;
		this->sogaIzqInicial = false;
	} else {
		pair<double,double> parXYAObtener = this->obtenerPosicionSoga(1);
		soga->setPosicionFinal(parXYAObtener.first,parXYAObtener.second);
		this->sogaDerUnida = soga;
		this->sogaDerInicial = false;
	}

}

void Polea::eliminarSoga(Elemento* soga) {
	cout << "soga a eliminar en " << this->getTipo()  << " con id: "  << soga->getIdElemento() << endl;
	if (this->sogaIzqUnida) {
		cout << "soga izq con id: " << this->sogaIzqUnida->getIdElemento()
				<< endl;

		if (this->sogaIzqUnida->getIdElemento() == soga->getIdElemento()){
			cout << "Elimino soga izquierda" << endl;
			this->sogaIzqUnida = NULL;
			this->sogaIzqInicial = false;
			cout << "Elimine soga izquierda unida" << endl;
			return;
		}
	}

	if (this->sogaDerUnida) {
		cout << "soga der con id: " << this->sogaDerUnida->getIdElemento()
				<< endl;

		if (this->sogaDerUnida->getIdElemento() == soga->getIdElemento()){
			cout << "Elimino soga derecha" << endl;
			this->sogaDerUnida = NULL;
			this->sogaDerInicial = false;
			return;
		}
	}
}

int Polea::seSuperpone(Elemento* elemento) {
	if (elemento->getTipo() == "soga")
		return 0;
	return 2;
}

void Polea::eliminarReferencias(list<Elemento*>* elementosCanvas) {

	pair<Elemento*, Elemento*> extremosSoga;
	pair<Elemento*, Elemento*> extremosSogaIzq, extremosSogaDer;

	if (this->sogaIzqUnida) {

		this->sogaIzqUnida->eliminarReferencias(elementosCanvas);
		delete this->sogaIzqUnida;
	}
	if (this->sogaDerUnida) {
		this->sogaDerUnida->eliminarReferencias(elementosCanvas);
		delete this->sogaDerUnida;
	}
}

int Polea::haySogaIzqUnida() {
	if (this->sogaIzqUnida)
		return 1;
	return 0;
}

int Polea::haySogaDerUnida() {
	if (this->sogaDerUnida)
		return 1;
	return 0;
}

bool Polea::esBasico() {
	return false;
}

bool Polea::sogaEnPosInicial() {
	return this->sogaIzqInicial;
}

bool Polea::sogaEnPosFinal() {
	return this->sogaDerInicial;

}

pair<Elemento*, Elemento*> Polea::obtenerExtremos() {
	pair<Elemento*, Elemento*> extremos;
	extremos.first = NULL;
	extremos.second = NULL;
	cout << "estoy en Polea con haySogaIzq: " << this->haySogaIzqUnida()
			<< " y hay soga derecha: " << this->haySogaDerUnida() << endl;

	if (this->sogaIzqUnida) {
		cout << "TIENE SOGA IZQ " << endl;
		extremos.first = this->sogaIzqUnida;
	}
	if (this->sogaDerUnida) {
		cout << "TIENE SOGA DER " << endl;
		extremos.second = this->sogaDerUnida;
	}
	return extremos;
}

void Polea::setSogaInicial(Elemento* elem) {
	this->sogaIzqUnida = elem;
}

void Polea::setSogaFinal(Elemento* elem) {
	this->sogaDerUnida = elem;

}

pair<double,double> Polea::obtenerPosicionSoga(int sentido){

	pair<double,double> posicionXYAObtener;

	// Soga izquierda
	if (sentido == 0){

		double angulo = this->getAnguloRotacion() - 360;

		double xTrasladado = -this->getAncho()/2 + this->getAncho() * PROPORCION_SOGA_POLEA_X_IZQ;
		double yTrasladado = this->getAlto()/2 - this->getAlto() * PROPORCION_SOGA_POLEA_Y;

		double xRotado = xTrasladado * cos(angulo * PI / 180.0) +
				yTrasladado	* sin(angulo * PI/ 180.0);
		double yRotado = -xTrasladado * sin(angulo * PI / 180.0)	+
				yTrasladado	* cos(angulo * PI/ 180.0);

		double xAObtener = this->getX() + this->getAncho()/2 + xRotado;
		double yAObtener = this->getY() + this->getAlto()/2  - yRotado;

		posicionXYAObtener.first = xAObtener;
		posicionXYAObtener.second = yAObtener;
	}
	// Soga derecha
	else if (sentido == 1){
		double angulo = 360 - this->getAnguloRotacion();

		double xTrasladado = -this->getAncho()/2 + this->getAncho() * PROPORCION_SOGA_POLEA_X_DER;
		double yTrasladado = this->getAlto()/2 - this->getAlto() * PROPORCION_SOGA_POLEA_Y;

		double xRotado = xTrasladado * cos(angulo * PI / 180.0) -
				yTrasladado	* sin(angulo * PI/ 180.0);
		double yRotado = xTrasladado * sin(angulo * PI / 180.0)	+
				yTrasladado	* cos(angulo * PI/ 180.0);

		double xAObtener = this->getX() + this->getAncho()/2 + xRotado;
		double yAObtener = this->getY() + this->getAlto()/2  - yRotado;

		posicionXYAObtener.first = xAObtener;
		posicionXYAObtener.second = yAObtener;
	}

	return posicionXYAObtener;
}



